#include "StdAfx.h"
#include "OrientedArc.h"
#include <limits>

#ifdef max
#undef max
#endif


OrientedArc::OrientedArc(const ContourDot& inDot, bool inReverse)
	:	mX(inDot.GetX()), mY(inDot.GetY()),
		mVx(inDot.GetVx()), mVy(inDot.GetVy()),
		mCurvature(inDot.GetCurvature())
{
	if (inReverse)
	{
		mVx = -mVx;
		mVy = -mVy;
		mCurvature = -mCurvature;
	}
}

	
bool OrientedArc::FindIntersection(const OrientedArc& inOtherArc,
	float inLimitForParameter,
	float& outX, float& outY, float& outMyParameter,
	float& outOtherParameter) const
{
	float dx = inOtherArc.mX - mX;
	float dy = inOtherArc.mY - mY;
	float distanceSquared = dx * dx + dy * dy;
	float c = mVx * inOtherArc.mVx + mVy * inOtherArc.mVy;
	float s = mVx * inOtherArc.mVy - mVy * inOtherArc.mVx;
	float dn1 = dy * mVx - dx * mVy;
	float dn2 = dy * inOtherArc.mVx - dx * inOtherArc.mVy;
	float kk = mCurvature * inOtherArc.mCurvature;
	float f = c + dn1 * inOtherArc.mCurvature - dn2 * mCurvature 
		- 0.5f * distanceSquared * kk;
	float q = 1 - f * f;

	if ( 0 >= q)
	{
		return false;
	}

	float atx = - dy * kk - mCurvature * inOtherArc.mVx + inOtherArc.mCurvature * mVx;
	float aty = dx * kk - mCurvature * inOtherArc.mVy + inOtherArc.mCurvature * mVy;
	float at2 = atx * atx + aty * aty;
	if (0.0f == at2)
	{
		if (0.0f < mCurvature || 0.0f < inOtherArc.mCurvature)
		{
			// the two circles with common center do not intersect
			return false;
		}
		// with zero curvatures, it is two lines with one intersection point.
		// (note in this case: s!=0 because q=|s|>0)
		float t = -dn2 / s;
		outX = mX + mVx * t;
		outY = mY + mVy * t;
		return CheckIntersectionPoint(inOtherArc, inLimitForParameter, outX,
			outY, outMyParameter, outOtherParameter);
	}

	float eq = (s >= 0 ? sqrt(q) : -sqrt(q));

	float dk2a = (inOtherArc.mCurvature * inOtherArc.mCurvature - 
		mCurvature * mCurvature) / (2.0f * at2);
	float d2dr = 0.5f * distanceSquared * kk + mCurvature * dn2 - 
		inOtherArc.mCurvature * dn1;
	float skka = s * kk / at2;
	float t1 = d2dr * (2.0f * c - d2dr) / (at2 * (s + eq));
	float x1 = 0.5f * (mX + inOtherArc.mX) + dk2a * dx - skka * dy -
		d2dr * (mVy * inOtherArc.mCurvature + inOtherArc.mVy * mCurvature) / at2 +
		atx * t1;
	float y1 = 0.5f * (mY + inOtherArc.mY) + dk2a * dy + skka * dx +
		d2dr * (mVx * inOtherArc.mCurvature + inOtherArc.mVx * mCurvature) / at2 +
		aty * t1;

	if (CheckIntersectionPoint(inOtherArc, inLimitForParameter, x1, y1,
			outMyParameter, outOtherParameter))
	{
		outX = x1;
		outY = y1;
		return true;
	}

	float deltaT = 2.0f * eq / at2;
	float x2 = x1 - atx * deltaT;
	float y2 = y1 - aty * deltaT;
	if (CheckIntersectionPoint(inOtherArc, inLimitForParameter, x2, y2,
			outMyParameter, outOtherParameter))
	{
		outX = x2;
		outY = y2;
		return true;
	}

	return false;
}

void OrientedArc::GetTangentVectorAtPoint(float inX, float inY,
		float& outVx, float& outVy) const
{
	outVx = mVx - mCurvature * (inY - mY);
	outVy = mVy + mCurvature * (inX - mX);
}

float OrientedArc::EquationValueAtPoint(float inX, float inY) const
{
	float dx = inX - mX;
	float dy = inY - mY;
	float squaredDistance = dx * dx + dy * dy;
	return mVy * dx - mVx * dy + 0.5f * mCurvature * squaredDistance;
}

float OrientedArc::ParameterAtPoint(float inX, float inY) const
{
	float dx = inX - mX;
	float dy = inY - mY;
	if (float squaredDistance = dx * dx + dy * dy)
	{
		return (mVx * dx + mVy * dy) / squaredDistance;
	}
	return std::numeric_limits<float>::max();
}

float OrientedArc::DiscrepancyWithArc(const OrientedArc& inOtherArc) const
{
	float dx = mX - inOtherArc.mX;
	float dy = mY - inOtherArc.mY;

	// approximately the gap between curves
	float d1 = EquationValueAtPoint(inOtherArc.mX, inOtherArc.mY);
	float d2 = inOtherArc.EquationValueAtPoint(mX, mY);

	return (d1 * d1 + d2 * d2) / (dx * dx + dy * dy);
}

//---------------------------------------------
bool OrientedArc::CheckIntersectionPoint(const OrientedArc& inOtherArc,
		float inLimitForParameter,
		float inX, float inY, float& outMyParameter,
		float& outOtherParameter) const
{
	float sMy = ParameterAtPoint(inX, inY);
	if (sMy < inLimitForParameter)
	{
		return false;
	}
	float sOther = inOtherArc.ParameterAtPoint(inX, inY);
	if (sOther < inLimitForParameter)
	{
		return false;
	}
	outMyParameter = sMy;
	outOtherParameter = sOther;
	return true;
}